#include "map_reader.h"
#include <cstdlib> // for getenv

using namespace tinyxml2;

MapStreamReader::MapStreamReader() : Node("map_stream_reader") {}

bool MapStreamReader::load_map_xmlfile() {
    XMLDocument doc;
    if (doc.LoadFile(kMapPath.c_str()) != XML_SUCCESS) {
        RCLCPP_ERROR(this->get_logger(), "load map xml file failed");
        return false;
    }

    root = doc.RootElement();
    if (!root) {
        RCLCPP_ERROR(this->get_logger(), "xml root is nullptr");
        return false;
    }

    // Find map ID based on robot ID
    std::string mapId = FindMapId(root);
    secondRoot = FindMapData(root, mapId);
    if (!secondRoot) {
        RCLCPP_ERROR(this->get_logger(), "xml secondRoot is nullptr");
        return false;
    }

    std::string point_element_name = "point";
    point_Node = secondRoot->FirstChildElement(point_element_name.c_str());
    pointsMap.clear();

    while (point_Node != nullptr) {
        try {
            int id = point_Node->IntAttribute("id");
            double x = point_Node->DoubleAttribute("x");
            double y = point_Node->DoubleAttribute("y");
            int navType = point_Node->IntAttribute("type");
            double angle = point_Node->DoubleAttribute("angle");
            int qrCodeId = -1;
            
            const char* attrValue = point_Node->Attribute("qrCodeId");
            if(attrValue != nullptr) {
                qrCodeId = point_Node->IntAttribute("qrCodeId");
            }
            
            RCLCPP_INFO(this->get_logger(), "id %d x %f y %f qrCodeId %d", id, x, y, qrCodeId);
            
            MapPointData MapPointData_tmp(id, x, y, angle, navType, qrCodeId);
            pointsMap[id] = MapPointData_tmp;
            
            RCLCPP_INFO(this->get_logger(), "cur map point(%d) is (%d, %f, %f)",
                       id, navType, x, y);
            
            point_Node = point_Node->NextSiblingElement(point_element_name.c_str());
        }
        catch (const std::exception &e) {
            RCLCPP_ERROR(this->get_logger(), "io load map point crash: %s", e.what());
        }
        catch (...) {
            RCLCPP_ERROR(this->get_logger(), "error: read xml crash");
        }
    }
    return true;
}

std::string MapStreamReader::FindMapId(XMLElement *root) {
    if (!root) {
        RCLCPP_ERROR(this->get_logger(), "FindMapId failed: root is null");
        return "";
    }

    try {
        const char* robot_id_env = std::getenv("ROBOT_ID");
        if (!robot_id_env) {
            RCLCPP_ERROR(this->get_logger(), "ROBOT_ID environment variable not set");
            return "";
        }
        
        std::string robot_id = std::string(robot_id_env);
        std::string group_name = "group";
        std::string agv_number_name = "agv_number";
        XMLElement *first_element = root->FirstChildElement(group_name.c_str());

        while (first_element != nullptr) {
            std::string cur_element_name = std::string(first_element->Name());
            if (group_name == cur_element_name) {
                std::string map_id = first_element->Attribute("mapId") ? first_element->Attribute("mapId") : "";
                XMLElement *first_agv_number = first_element->FirstChildElement(agv_number_name.c_str());
                while (first_agv_number != nullptr) {
                    std::string cur_number_name = std::string(first_agv_number->Name());
                    if (agv_number_name == cur_number_name) {
                        std::string id = first_agv_number->Attribute("id") ? first_agv_number->Attribute("id") : "null_agv_id";
                        if (id == robot_id) {
                            RCLCPP_INFO(this->get_logger(), "find cur map id ok mapId: %s, cur_robot_id: %s", 
                                      map_id.c_str(), robot_id.c_str());
                            return map_id;
                        }
                    }
                    first_agv_number = first_agv_number->NextSiblingElement();
                }
            }
            first_element = first_element->NextSiblingElement();
        }
    }
    catch (...) {
        RCLCPP_ERROR(this->get_logger(), "error: FindMapId crash");
    }
    return "map_id_null";
}

XMLElement *MapStreamReader::FindMapData(XMLElement *root, const std::string &target_map_id) {
    if (!root) {
        RCLCPP_ERROR(this->get_logger(), "FindMapData failed: root is null");
        return nullptr;
    }

    try {
        std::string map_name = "map";
        XMLElement *first_element = root->FirstChildElement(map_name.c_str());

        while (first_element != nullptr) {
            std::string cur_element_name = std::string(first_element->Name());
            if (map_name == cur_element_name) {
                std::string map_id = first_element->Attribute("id") ? first_element->Attribute("id") : "";
                if (target_map_id == map_id) {
                    RCLCPP_INFO(this->get_logger(), "find target_map_id ok: target_map_id: %s", 
                               target_map_id.c_str());
                    return first_element;
                }
            }
            first_element = first_element->NextSiblingElement();
        }
    }
    catch (...) {
        RCLCPP_ERROR(this->get_logger(), "error: FindMapData crash");
    }
    return nullptr;
}